﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;

namespace MotionPlanning
{
    public class MotionPlanningUtils
    {
        public const double PI_double = Math.PI+Math.PI;
        static public void vector_set(double[] q_src, ref double[] q_dst)
        {
            if(q_dst==null)
                q_dst=new double[q_src.Length];
            for (int i = 0; i < q_src.Length; ++i)
                q_dst[i] = q_src[i];
        }
        static public void vector_add(double[] q_1, double[] q_2, ref double[] q_out)
        {
            if (q_1 == null)
                throw new NullReferenceException();
            if (q_2 == null)
                throw new NullReferenceException();
            if (q_1.Length != q_2.Length)
                throw new Exception("vector dimension mismatch.");
            if (q_out == null)
                q_out = new double[q_1.Length];
            for (int i = 0; i < q_1.Length; ++i)
                q_out[i] = q_1[i] + q_2[i];
        }
        static public void vector_kTimes(double[] v, double k, ref double[] v_out)
        {
            for (int i = 0; i < v.Length; ++i)
                v_out[i] = v[i] * k;
        }
        static public void vector_linear(double[] v, double k, double b, ref double[] v_out)
        {
            for (int i = 0; i < v.Length; ++i)
                v_out[i] = v[i] * k + b;
        }
        static public void vector_vlinear(double[] v, double k, double[] b, ref double[] v_out)
        {
            for (int i = 0; i < v.Length; ++i)
                v_out[i] = v[i] * k + b[i];
        }
        static public string ArrayToString(double[] array)
        {
            string content = "";
            for (int i = 0; i < array.Length; ++i)
            {
                content += array[i];
                if (i < array.Length - 1)
                    content += ", ";
            }
            return content;
        }
        static public string ArrayToString(int[] array)
        {
            string content = "";
            for (int i = 0; i < array.Length; ++i)
            {
                content += array[i];
                if (i < array.Length - 1)
                    content += ", ";
            }
            return content;
        }
        static public bool StringToBool(string textString)
        {
            return Boolean.Parse(textString.Trim());
        }
        static public double[] StringToDoubleArray(string textString)
        {
            string[] separaters = new string[] { ", ", ",", " ," };
            string[] entries = textString.Split(separaters, StringSplitOptions.RemoveEmptyEntries);
            double[] array = new double[entries.Length];
            for (int i = 0; i < array.Length; ++i)
            {
                array[i] = Double.Parse(entries[i]);
            }
            return array;
        }
        static public int[] StringToIntArray(string textString)
        {
            string[] separaters = new string[] { ", ", ",", " ," };
            string[] entries = textString.Split(separaters, StringSplitOptions.RemoveEmptyEntries);
            int[] array = new int[entries.Length];
            for (int i = 0; i < array.Length; ++i)
            {
                array[i] = int.Parse(entries[i]);
            }
            return array;
        }
        public static void ColorToHSV(Color color, out double hue, out double saturation, out double value)
        {
            int max = Math.Max(color.R, Math.Max(color.G, color.B));
            int min = Math.Min(color.R, Math.Min(color.G, color.B));
            hue = color.GetHue();
            saturation = (max == 0) ? 0 : 1d - (1d * min / max);
            value = max / 255d;
        }
        public static Color ColorFromHSV(double hue, double saturation, double value)
        {
            int hi = Convert.ToInt32(Math.Floor(hue / 60)) % 6;
            double f = hue / 60 - Math.Floor(hue / 60);
            value = value * 255;
            int v = Convert.ToInt32(value);
            int p = Convert.ToInt32(value * (1 - saturation));
            int q = Convert.ToInt32(value * (1 - f * saturation));
            int t = Convert.ToInt32(value * (1 - (1 - f) * saturation));
            if (hi == 0)
                return Color.FromArgb(255, v, t, p);
            else if (hi == 1)
                return Color.FromArgb(255, q, v, p);
            else if (hi == 2)
                return Color.FromArgb(255, p, v, t);
            else if (hi == 3)
                return Color.FromArgb(255, p, q, v);
            else if (hi == 4)
                return Color.FromArgb(255, t, p, v);
            else
                return Color.FromArgb(255, v, p, q);
        }
        public static Color Cost2Color(double cost_max, double cost)
        {
            if (cost > cost_max)
                return Color.Black;
            double hue = cost / cost_max * 310;
            return ColorFromHSV(hue, 1, 1);
        }
    }
}
